#!/bin/sh

DAEMON="motors-daemon"
DAEMON_ARGS="-d -p"

. /usr/share/common

ensure_command "motors"

set_motor_phases() {
	if [ "$(echo $2 | wc -w)" -lt 4 ]; then
		echo_error "Missing phase for ${1}motor"
		exit 1
	fi

	local i=1
	for gpio in $2; do
		eval "${1}st${i}=$gpio"
		i=$((i + 1))
	done
}

home_motors() {
	echo_info "Home motors"
	motors -r
}

set_motors_speed() {
	echo_info "Set motors speed to $motor_speed"
	motors -s $1
}

position_motors() {
	local x y

	if [ -n "$motor_pos_0" ]; then
		x=$(echo $motor_pos_0 | cut -d, -f1)
		y=$(echo $motor_pos_0 | cut -d, -f2)
	else
		x=$((motor_maxstep_h / 2))
		y=$((motor_maxstep_v / 2))
	fi

	echo_info "Move to $x,$y"
	motors -d h -x $x -y $y
}

start() {
	echo_title "Setting up motors"

	if [ -z "$motor" ]; then
		if [ -z "$gpio_motor_h$gpio_motor_v" ]; then
			echo_error "No motor pins defined"
		fi

		if [ -z "$gpio_motor_h" ]; then
			echo_warning "gpio_motor_h is not set"
		fi

		if [ -z "$gpio_motor_v" ]; then
			echo_warning "gpio_motor_v is not set"
		fi

		if [ -z "$gpio_motor_switch" ]; then
			echo_warning "gpio_motor_switch is not set"
		fi
	fi

	if [ -z "$motor_maxstep_h" ]; then
		echo_warning "motor_maxstep_h is not set, set to 4000"
		motor_maxstep_h=4000
	fi

	if [ -z "$motor_maxstep_v" ]; then
		echo_warning "motor_maxstep_v is not set, set to 2000"
		motor_maxstep_v=2000
	fi

	if [ -z "$motor_speed_h" ]; then
		echo_warning "motor_speed_h is not set, set to 900"
		motor_speed_h=900
	fi

	if [ -z "$motor_speed_v" ]; then
		echo_warning "motor_speed_v is not set, set to 900"
		motor_speed_v=900
	fi

	if [ -z "$motor_speed" ]; then
		motor_speed=900
	fi

	if grep -qE "^motor" /proc/modules; then
		echo_info "Module motor already loaded."
	else
		modprobe_args="hmaxstep=$motor_maxstep_h vmaxstep=$motor_maxstep_v"

		if [ -z "$motor" ]; then
			modprobe_args="$modprobe_args tcu_channels=2"
			echo_info "Set GPIO per motor phase"

			set_motor_phases "h" "$gpio_motor_h"
			set_motor_phases "v" "$gpio_motor_v"

			modprobe_args="$modprobe_args hst1=$hst1 hst2=$hst2 hst3=$hst3 hst4=$hst4"

			[ -z "$vst1" ] && vst1="$hst1"
			[ -z "$vst2" ] && vst2="$hst2"
			[ -z "$vst3" ] && vst3="$hst3"
			[ -z "$vst4" ] && vst4="$hst4"

			modprobe_args="$modprobe_args vst1=$vst1 vst2=$vst2 vst3=$vst3 vst4=$vst4"

			if echo "$gpio_motor_switch" | grep -qE '^[0-9]+$'; then
				modprobe_args="$modprobe_args motor_switch_gpio=$gpio_motor_switch"
			fi

			if [ "true" = "$gpio_motor_invert" ]; then
				modprobe_args="$modprobe_args invert_gpio_dir=1"
			fi
		fi

		echo_info "Load motor module with parameters: $modprobe_args"

		if ! modprobe motor $modprobe_args; then
			echo_error "Failed to load motor module"
			exit 1
		fi
	fi

	start_daemon
	# FIXME: daemon should be reporting running state from the very first moment
	sleep 1

	# FIXME: motors may have different speeds for pan and tilt
	set_motors_speed $motor_speed_h

	if [ "true" = "$motor_disable_homing" ]; then
		echo_info "Homing disabled"
	else
		home_motors
		position_motors
	fi
}

stop() {
	echo_title "Stopping motors"

	stop_daemon

	exit 0
	# FIXME: module crashes on unloading
	if ! rmmod motor; then
		echo_error "Failed to unload motor module."
		exit 1
	fi
}

case "$1" in
	start)
		start
		;;
	stop)
		stop
		;;
	restart)
		stop
		sleep 1
		start
		;;
	*)
		echo "Usage: $0 {start|stop|restart}"
		exit 1
		;;
esac

exit 0
